#include "mpc.h"
#include "config.h"
#include "esp_log.h"

#include "config.h"
#ifndef USE_SYS_MEM
#include "m_memory.h"
extern uint8_t memory_pool[16 * 1024];
extern MemoryPoolManager pool;
#endif

static const char * TAG = "mpc";

uint16_t get_protocol_packet_size(const protocol_packet_t *pkt) {
    if (!pkt) return 0;  // 检查空指针
    return PACKET_TOTAL_SIZE(pkt->length);
}

packet_parse_result_t parse_protocol_packet(uint8_t *buf, uint16_t buf_len, protocol_packet_t **out_pkt) {
    if (buf_len < 6) return PACKET_INCOMPLETE;
    if (buf[0] != HEADER1 || buf[1] != HEADER2) {
        return PACKET_ERROR;
    }
    uint8_t srcdest = buf[2];
    uint8_t cmd = buf[3];
    uint8_t subcmd = buf[4];
    uint8_t len = buf[5];
#ifdef ENABLE_CRC_CHECK
    uint8_t total_size = 6 + len + 2;
#else
    uint8_t total_size = 6 + len;
#endif
    if (buf_len < total_size) {
        return PACKET_INCOMPLETE;
    }
    
#ifdef ENABLE_CRC_CHECK // 提取 CRC（如果开启）
    uint16_t crc_received = *(uint16_t *)(buf + 6 + len);

    // 构造用于校验的数据段：cmd + len + data
    uint8_t checksum_data[4 + len];
    checksum_data[0] = srcdest;
    checksum_data[1] = cmd;
    checksum_data[2] = subcmd;
    checksum_data[3] = len;
    memcpy(checksum_data + 4, buf + 6, len);

    uint16_t crc_calculated = crc16(checksum_data, sizeof(checksum_data));

    if (crc_calculated != crc_received) {
        return PACKET_ERROR;
    }
// #else
//     // 不做 CRC 校验
//     (void)buf;  // 避免未使用警告
#endif
    // 分配内存并复制数据
#ifdef USE_SYS_MEM
    protocol_packet_t *pkt = malloc(total_size);
#else
    protocol_packet_t *pkt = (protocol_packet_t*)mempool_alloc(&pool, total_size);
#endif
    if (!pkt) return PACKET_ERROR;
    pkt->header1 = HEADER1;
    pkt->header2 = HEADER2;
    pkt->srcdest = srcdest;
    pkt->cmd = cmd;
    pkt->subcmd = subcmd;
    pkt->length = len;
    memcpy(pkt->data, buf + 6, len);
#ifdef ENABLE_CRC_CHECK
    PACKET_CRC(pkt) = crc_received;
#else
    // 如果不启用 CRC 校验，则跳过 CRC 字段赋值
    (void)total_size;
#endif
    *out_pkt = pkt;
    return PACKET_OK;
}

protocol_packet_t *protocol_build_packet(uint8_t srcdest, uint8_t cmd, uint8_t subcmd, const uint8_t *data, uint8_t len)
{
    uint16_t total_size = PACKET_TOTAL_SIZE(len);
#ifdef USE_SYS_MEM
    protocol_packet_t *pkt = malloc(total_size);
#else
    protocol_packet_t *pkt = (protocol_packet_t*)mempool_alloc(&pool, total_size);
#endif
    if (!pkt) {
        return NULL;  // 内存分配失败
    }
    pkt->header1 = HEADER1;
    pkt->header2 = HEADER2;
    pkt->srcdest = srcdest;
    pkt->cmd = cmd;
    pkt->subcmd = subcmd;
    pkt->length = len;
    if (data && len > 0) {
        memcpy(pkt->data, data, len);
    }
#ifdef ENABLE_CRC_CHECK
    // 构造校验数据段：cmd + subcmd + len + data
    uint8_t checksum_data[3 + len];
    checksum_data[0] = cmd;
    checksum_data[1] = subcmd;
    checksum_data[2] = len;
    if (data && len > 0) {
        memcpy(checksum_data + 3, data, len);
    }

    uint16_t crc = crc16(checksum_data, sizeof(checksum_data));
    PACKET_CRC(pkt) = crc;
#endif
    // ESP_LOG_BUFFER_HEX(TAG, pkt, pkt->length + 6);
    return pkt;
}